﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using LDriver;

namespace LDriver.Protocols
{
    public class LidarProtocol : ICommunicationProtocol
    {
        public List<Command> Commands { get; private set; }
        public List<ISurvey> Surveys { get; private set; }

        /// <summary>
        /// 
        /// </summary>
        public void Initialize()
        {
            // Gestire init dei commands
            Commands = new List<Command>
                       {
                           new Command("START", new byte[] {0xA5, 0x20}, new byte[] {0xA5, 0x5A, 0x05, 0x00,0x00,0x40,0x81}, 5 ),
                           new Command("STOP", new byte[] {0xA5, 0x25}, new byte[] {0xA5, 0x5A}, 0 ),
                           new Command("Reset", new byte[] {0xA5, 0x40}, new byte[] {0x41}, 6 ),
                           new Command("GetInfo", new byte[] {0xA5, 0x50}, new byte[] {0x41}, 6 ),
                           new Command("GetHealt", new byte[] {0xA5, 0x52}, new byte[] {0x41}, 6 )
                       };
            Surveys = new List<ISurvey> { };

        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="commandName"></param>
        /// <returns></returns>
        public Command GetCommand(string commandName)
        {
            return Commands.FirstOrDefault(command => command.Name == commandName);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="IncomingData"></param>
        public void AnalyzeDataReceived(byte[] IncomingData)
        {
        }

        public bool AnalyzePacket(Command command, Packet packet)
        {
            try
            {

            

            // A seconda del comando utilizzato spacchetto il pacchetto arrivato, in base
            // alle specifiche.
            if (command.Name=="START")
            {
                // Il pacchetto per il comando START è composto da 5 byte, escludendo l'header.
                // byte[0] Quality
                // byte[1] angle_q6[6:0]
                // byte[2] angle_q6[14:7]
                // byte[3] distance_q2[7:0]
                // byte[4] distance_q2[7:0]
                
                // Del byte[0] verifico il primo bit a sinistra:
                // quando 1 significa che devo pulire il visualizer perchè indica
                // una nuova registrazione, quindi fondamentalmente quando
                // è dispari

                LidarSurvey ls = new LidarSurvey
                {
                    Quality = (packet.Content[0] >> 2),
                    S = (packet.Content[0] & 0x1),
                    Angle = (double)(((packet.Content[2] << 8) + packet.Content[1]) >> 1) / 64.00,
                    Distance = (double)(((packet.Content[4] << 8) + packet.Content[3])) / 40.00,
                   
                };

                //if (!(m.Angle >= 0 && m.Angle <= 361))
                //{
                //    return false;
                //}

                // TODO: Verifica Correttezza valore 10. Da creare parametro per setting da interfaccia
                //if (ls.Quality>10)
                //{
                    Surveys.Add(ls);
                    return true;
                //}

                return false;

            }
            else if (command.Name == "STOP")
            {
                // Nessun pacchetto da gestire per il comando STOP
                return true;
            }
            else
            {
                return false;
            }
            }
            catch (Exception)
            {

                throw;
            }
        }

    }
}
